// -*- coding: utf-8 -*-
#ifndef CHASSIS_H
#define CHASSIS_H

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/PositionTarget.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistStamped.h>
#include <math.h>
#include <std_srvs/Trigger.h>
#include <std_msgs/ColorRGBA.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/transform_datatypes.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Float32.h>
#include "std_msgs/Float32MultiArray.h"
#include "std_msgs/Float64MultiArray.h"
#include <std_msgs/String.h>
#include <stdlib.h>

extern ros::Subscriber state_sub;
extern ros::Subscriber pose_sub;
extern ros::Subscriber vel_sub;
extern ros::Publisher local_pose_pub;
extern ros::Publisher local_vel_pub;
extern ros::Publisher state_pub;
extern ros::Publisher set_raw_pub;
extern ros::Publisher state_message_pub;
extern ros::ServiceClient arming_client;
extern ros::ServiceClient set_mode_client;

extern int land_distance;
extern bool cur_state;
extern bool hold_reset_flag;
extern bool rotation_reset_flag;
extern bool velocity_reset_flag;
extern mavros_msgs::State current_state;
extern geometry_msgs::PoseStamped current_pose;
extern geometry_msgs::TwistStamped current_vel;

bool state_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
void reset_hold_state();
void reset_rotation_state();
void state_cb(const mavros_msgs::State::ConstPtr &msg);
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
void vel_cb(const geometry_msgs::TwistStamped::ConstPtr &msg);

class move_map
{
private:
    double radius = 1.0;
    double speed = 0.2;
    double angular_speed = speed / radius;
    double angle = 0.0;
    double rotation_angle;
    double last_angle = 0;

    double func(double error, double A, double B);

public:
    int fly_to_target(geometry_msgs::PoseStamped target_pose, ros::Publisher &local_pose_pub, ros::Rate &rate, double speed);
    void pose_velocity_ctrl(double vx, double vy, double vz, double yaw_rate, ros::Rate &rate);
    void reset_velocity_control();
    void hold(ros::Publisher &local_pose_pub, ros::Rate &rate);
    void reset_hold();
    int performPoleSurrounding(float pole_x, float pole_y, float radius, float height, bool clockwise = true);
    bool performRotation(double targetYaw, ros::Rate &rate);
    int performCircleFlight(double centerX, double centerY, double centerZ, double radius, double elapsedTime, ros::Rate &rate);
    bool performSafeLanding(double descent_rate, ros::Rate &rate);
    bool isReachedTarget(double target_x, double target_y, double target_z, double tolerance = 0.3);
    bool performOffboardEnable(ros::Rate &rate);
    bool isOffboardEnabled();
    bool isVehicleArmed();
};

#endif
